2. Mathematical Model¶
Optimization Goal¶
Our goal is to optimize each point along a path such that the path becomes:
- As smooth as possible: Reduce sharp turns to minimize curvature along the path.
- Close to the original path: Keep the optimized path close to the initial path, ensuring that it maintains the general shape.
Smoothness Loss¶
The smoothness loss is designed to minimize the curvature of the path by using the second-order difference of the path points. This second-order difference approximates the second derivative of the path function, which reflects the path’s curvature.
For a continuous function $f(x)$, its second derivative at a point $x$, denoted $f''(x)$, can be approximated using Taylor expansion. Suppose we have three discrete points $x_{i-1}$, $x_i$, $x_{i+1}$, each separated by a distance $h$. We perform Taylor expansions for $f(x_{i+1})$ and $f(x_{i-1})$:
Taylor Expansion of $f(x_{i+1})$
$$
f(x_{i+1}) = f(x_i) + h f'(x_i) + \frac{h^2}{2} f''(x_i) + \frac{h^3}{6} f'''(x_i) + \cdots
$$
Taylor Expansion of $f(x_{i-1})$
$$
f(x_{i-1}) = f(x_i) - h f'(x_i) + \frac{h^2}{2} f''(x_i) - \frac{h^3}{6} f'''(x_i) + \cdots
$$
Adding these two expansions cancels the first derivative term $f'(x_i)$:
$$
f(x_{i+1}) + f(x_{i-1}) = 2f(x_i) + h^2 f''(x_i) + \text{higher order terms}
$$
Rearranging terms, we approximate $f''(x_i)$:
$$
f''(x_i) \approx \frac{f(x_{i+1}) - 2f(x_i) + f(x_{i-1})}{h^2}
$$
Thus, when $h = 1$,the second derivative can be approximated by the discrete points $x_{i-1}$, $x_i$, $x_{i+1}$:
$$
f(x_{i+1}) - 2f(x_i) + f(x_{i-1})
$$
In the path smoothing problem, we aim to minimize the curvature at each path point. Thus, we define the smoothness loss as the sum of squared second-order differences:
$$
\text{smoothness\_loss} = \sum_{i=2}^{N-1} \left((x[i-1] - 2x[i] + x[i+1])^2 + (y[i-1] - 2y[i] + y[i+1])^2\right)
$$
Proximity Loss¶
The proximity loss ensures that the optimized path remains close to the original path. Let the coordinates of the original path be $(x_0[i], y_0[i])$, and the optimized path points be $(x[i], y[i])$. The proximity loss is defined as the sum of squared Euclidean distances between each optimized point and its corresponding original point:
$$
\text{proximity\_loss} = \sum_{i=1}^N \left((x[i] - x_0[i])^2 + (y[i] - y_0[i])^2\right)
$$
constraints¶
To ensure that the path begins and ends at the specified locations, we introduce fixed constraints for the start and end points. These constraints anchor the optimized path to the original path's starting and ending points, ensuring that any adjustments made through optimization do not alter the intended entry and exit positions.
For a given path with coordinates $(x_0[1], y_0[1])$ as the start point and $(x_0[N], y_0[N])$ as the end point, the fixed start and end constraints are defined as follows:
$$
x[1] = x_0[1], \quad y[1] = y_0[1]
$$$$
x[N] = x_0[N], \quad y[N] = y_0[N]
$$
Obstacle Avoidance (if applicable): Additional constraints can be included to ensure that path points are within feasible (obstacle-free) regions.
Model Summary¶
Objective Function¶
We aim to minimize the weighted total loss:
$$
\min \quad \alpha \times \text{smoothness\_loss} + \beta \times \text{proximity\_loss}
$$
where:
- $\alpha$ is the weight for the smoothness loss, encouraging the path to avoid sharp turns.
- $\beta$ is the weight for the proximity loss, encouraging the path to stay close to the original path.
Decision Variables¶
The decision variable $x$ represents the optimized $x$- and $y$-coordinates of each path point.
$$
x = [x[1], y[1], x[2], y[2], \ldots, x[N], y[N]]
$$
Constraints¶
$$
x[1] = x_0[1], \quad y[1] = y_0[1]
$$$$
x[N] = x_0[N], \quad y[N] = y_0[N]
$$
To simplify solving this optimization problem, we convert it to a standard Quadratically Constrained Quadratic Programming (QCQP) form:
$$
\min_x \quad \frac{1}{2} x^T P_0 x + q_0^T x + r_0
$$
more detailed will be added soon¶
